//
// Created by lijia on 2021/1/12.
//

#include "joystick.h"
#include "stm32f1xx_hal.h"

static JoystickState state;
static JoystickConfig config = {
		.xBias = 0,
		.yBias = 0,
		.xDeadBand = 100,
		.yDeadBand = 100,
		.xNeutral = 1650,
		.yNeutral = 1650,
};
static ADC_HandleTypeDef *hwnd;
static uint16_t adc[2] = {0, 0};
static uint16_t adcX, adcY;
static volatile uint8_t isCalibrating = 0, hasConversionStarted = 0;

static inline uint8_t abs(int16_t x) {
	return x > 0 ? x : -x;
}

void joystick_init(ADC_HandleTypeDef *handle) {
	state.x = 0;
	state.y = 0;
	hwnd = handle;
	HAL_ADCEx_Calibration_Start(hwnd);
	HAL_ADC_Start_DMA(hwnd, (uint32_t *) adc, 2);
}

static void joystick_process() {
	if (isCalibrating) {
		static uint16_t calibratedX = 0, calibratedY = 0;
		static uint8_t tickCount = 0;
		if (++tickCount > 200) {
			isCalibrating = 0;
			tickCount = 0;
		}
		calibratedX = ((calibratedX * 8) + (adcX * 2)) / 10;
		calibratedY = ((calibratedY * 8) + (adcY * 2)) / 10;
		config.xNeutral = calibratedX;
		config.yNeutral = calibratedY;
	} else {
		state.x = (int16_t) (adcX - config.xNeutral);
		state.y = (int16_t) (adcY - config.yNeutral);
	}
}

void joystick_calibrate_neutral() {
	isCalibrating = 1;
}

uint8_t joystick_is_calibrating() {
	return isCalibrating;
}

const JoystickState *joystick_get() {
	return &state;
}

void joystick_tick() {
	int16_t xDiff = state.x + config.xBias;
	int16_t yDiff = state.y + config.yBias;
	if (abs(xDiff) > config.xDeadBand || abs(yDiff) > config.yDeadBand) {
		if (abs(xDiff) > abs(yDiff)) {
			if (xDiff > 0) {
				state.direction = JOYSTICK_DIR_SOUTH;
			} else {
				state.direction = JOYSTICK_DIR_NORTH;
			}
		} else {
			if (yDiff > 0) {
				state.direction = JOYSTICK_DIR_EAST;
			} else {
				state.direction = JOYSTICK_DIR_WEST;
			}
		}
	} else {
		state.direction = 0;
	}
}

void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef *_) {
	adcX = adc[0];
	adcY = adc[1];
	joystick_process();
	HAL_ADC_Start_DMA(_, (uint32_t *) adc, 2);
}
